/**
 * @file movement.c
 * @brief universal movement functions.
 */

#include <allegro5/base.h>

#include <movement.h>
#include <math.h>

void forward(float *x, float *y, float speed, float heading) {
  *x += speed * sinf(heading);
  *y -= speed * cosf(heading);
}

void accel(float *speed) {
  if (*speed < SPEED_LIMIT_HIGH) {
    *speed += SPEED_ACCEL;
  }
}
void brake(float *speed) {
  if (*speed < SPEED_LIMIT_LOW) {
    *speed = SPEED_LIMIT_LOW;
  }
  if (*speed > SPEED_LIMIT_LOW) {
    *speed -= SPEED_BRAKE;
  }
}
void inertion(float *speed) {
  if (*speed > SPEED_LIMIT_EPSILON) {
    *speed -= SPEED_INERTION;
  }
  if (*speed < SPEED_LIMIT_EPSILON) {
    *speed += SPEED_INERTION;
  }
}

float normolize(float angle) {
  return (atan2(sin(angle), cos(angle)));
}

void left(float *arg) {
  *arg -= TURN_RATE;
}
void right(float *arg) {
  *arg += TURN_RATE;
}

float gelu(float x) {
  x = (0.5 * x * (1 + tanhf(sqrtf(2.0f/ALLEGRO_PI) * (x + 0.044715*powf(x, 3.0f)))));
  return x;
}
